#include <hid.h>
#include <hiduniversal.h>
#include <usbhub.h>

#include "hidjoystickrptparser.h"

#define AUTO 200
#define LEDPIN 47

USB Usb;
USBHub Hub(&Usb);
HIDUniversal Hid(&Usb);
JoystickEvents JoyEvents;
JoystickReportParser Joy(&JoyEvents);
uint16_t prev_btns;
uint16_t prev_hats;
int select_state;
int first_run;
//button masks
uint16_t btn_1_msk = 0b1;
uint16_t btn_2_msk = 0b10;
uint16_t btn_3_msk = 0b100;
uint16_t btn_4_msk = 0b1000;
uint16_t btn_5_msk = 0b10000;
uint16_t btn_6_msk = 0b100000;
uint16_t btn_7_msk = 0b1000000;
uint16_t btn_8_msk = 0b10000000;
uint16_t btn_9_msk = 0b100000000;
uint16_t btn_10_msk = 0b1000000000;
uint16_t btn_11_msk = 0b10000000000;
uint16_t btn_12_msk = 0b100000000000;

ISR(TIMER5_COMPA_vect){
  digitalWrite(LEDPIN, !digitalRead(LEDPIN));
}

void turn_timer_on(){
  TCCR5B |= (1 << WGM12);
  TCCR5B |= (1 << CS10);
  TCCR5B |= (1 << CS12);
  TIMSK5 |= (1 << OCIE5A);
}

void turn_timer_off(){
  TCCR5B &= (0 << WGM12);
  TCCR5B &= (0 << CS10);
  TCCR5B &= (0 << CS12);
  TIMSK5 &= (0 << OCIE5A);
}

void setup() {
  
  Serial.begin(115200);
  while (!Serial); // Wait for serial port to connect - used on Leonardo, Teensy and other boards with built-in USB CDC serial connection
  Serial.println("Start");
  pinMode(LEDPIN, OUTPUT);
  cli();
  TCCR5A = 0;
  TCCR5B = 0;
  OCR5A = 7812;
  turn_timer_on();
  sei();
  
  if (Usb.Init() == -1)
    Serial.println("OSC did not start.");

  delay(200);

  if (!Hid.SetReportParser(0, &Joy))
    ErrorMessage<uint8_t > (PSTR("SetReportParser"), 1);
  
  select_state = AUTO;
  first_run = 2;
}

void loop() {
        Usb.Task();
        if((prev_btns != Joy.oldButtons) || (prev_hats != Joy.oldHat)){
          if(first_run <= 0){
            if(Joy.oldButtons & btn_1_msk){
              Serial.println("Button 1 Down!!");
              Serial.println(Joy.oldButtons,BIN);
            }
            if(Joy.oldButtons & btn_2_msk){Serial.println("Button 2 Down!!");Serial.println(Joy.oldButtons,BIN);}
            if(Joy.oldButtons & btn_3_msk){
              Serial.println("Button 3 Down!!");
              Serial.println(Joy.oldButtons,BIN);
              turn_timer_on();
            }
            if(Joy.oldButtons & btn_4_msk){
              Serial.println("Button 4 Down!!");
              Serial.println(Joy.oldButtons,BIN);
              turn_timer_off();
            }
            if(Joy.oldButtons & btn_5_msk){Serial.println("Button 5 Down!!");Serial.println(Joy.oldButtons,BIN);}
            if(Joy.oldButtons & btn_6_msk){Serial.println("Button 6 Down!!");Serial.println(Joy.oldButtons,BIN);}
            if(Joy.oldButtons & btn_7_msk){
              //Serial.println("Button 7 Down!!");
              //Serial.println(Joy.oldButtons,BIN);
              Serial.print("select_state - AUTO = ");
              Serial.println((select_state - AUTO),DEC);
              Serial.print("(select_state - AUTO) - 1 = ");
              int temp = select_state-AUTO-1;
              Serial.println(temp,DEC);
              Serial.print("((select_state - AUTO) - 1) % 3 = ");
              if(temp == -1)temp=2;
              Serial.println((temp % 3),DEC);
              Serial.print("AUTO + (((select_state - AUTO) - 1) % 3) = ");
              select_state = AUTO + (temp % 3);
              Serial.println(select_state, DEC);
            }
            if(Joy.oldButtons & btn_8_msk){
              Serial.println("Button 8 Down!!");
              Serial.println(Joy.oldButtons,BIN);
              select_state = AUTO + (((select_state - AUTO) + 1) % 3);
              Serial.println(select_state, DEC);
            }
            if(Joy.oldButtons & btn_9_msk){Serial.println("Button 9 Down!!");Serial.println(Joy.oldButtons,BIN);}
            if(Joy.oldButtons & btn_10_msk){Serial.println("Button 10 Down!!");Serial.println(Joy.oldButtons,BIN);}
            if(Joy.oldButtons & btn_11_msk){Serial.println("Button 11 Down!!");Serial.println(Joy.oldButtons,BIN);}
            if(Joy.oldButtons & btn_12_msk){Serial.println("Button 12 Down!!");Serial.println(Joy.oldButtons,BIN);}
            if(Joy.oldHat == 0){Serial.println("UP");Serial.println(Joy.oldHat,BIN);}
            if(Joy.oldHat == 2){Serial.println("RIGHT");Serial.println(Joy.oldHat,BIN);}
            if(Joy.oldHat == 4){Serial.println("DOWN");Serial.println(Joy.oldHat,BIN);}
            if(Joy.oldHat == 6){Serial.println("LEFT");Serial.println(Joy.oldHat,BIN);}
          }else{
            first_run--;
          }
        }
        prev_btns = Joy.oldButtons;
        prev_hats = Joy.oldHat;
}

